#!/bin/bash

# 服务文件检查脚本
# 用于检查 config.yaml 中定义的所有服务对应的执行文件是否存在

# 获取脚本所在目录
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

# 设置颜色输出
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
PURPLE='\033[0;35m'
CYAN='\033[0;36m'
NC='\033[0m' # No Color

echo -e "${BLUE}======================================${NC}"
echo -e "${BLUE}    ROS 服务文件检查器${NC}"
echo -e "${BLUE}======================================${NC}"

# 配置文件路径
CONFIG_FILE="$SCRIPT_DIR/config/config.yaml"

# 检查配置文件是否存在
if [ ! -f "$CONFIG_FILE" ]; then
    echo -e "${RED}错误：配置文件不存在！${NC}"
    echo -e "${YELLOW}期望路径：$CONFIG_FILE${NC}"
    exit 1
fi

echo -e "${GREEN}✓ 配置文件存在：$CONFIG_FILE${NC}"

# 检查必要的工具
check_dependencies() {
    local missing_deps=()
    
    # 检查python3
    if ! command -v python3 &> /dev/null; then
        missing_deps+=("python3")
    fi
    
    # 检查yq (用于解析YAML)
    if ! command -v yq &> /dev/null; then
        echo -e "${YELLOW}警告：yq未安装，将使用Python解析YAML${NC}"
    fi
    
    if [ ${#missing_deps[@]} -ne 0 ]; then
        echo -e "${RED}错误：缺少必要的依赖：${missing_deps[*]}${NC}"
        exit 1
    fi
}

# 检查ROS环境
check_ros_environment() {
    if [ -z "$ROS_DISTRO" ]; then
        echo -e "${YELLOW}警告：ROS环境未配置，某些检查可能不准确${NC}"
        echo -e "${YELLOW}建议先运行：${NC}"
        echo "source /opt/ros/noetic/setup.bash"
        echo "source ~/catkin_ws/devel/setup.bash"
        return 1
    else
        echo -e "${GREEN}✓ ROS环境已配置 (ROS $ROS_DISTRO)${NC}"
        return 0
    fi
}

# 展开波浪号路径
expand_path() {
    local path="$1"
    # 展开 ~ 为用户主目录
    if [[ "$path" =~ ^~/ ]]; then
        path="${HOME}/${path#~/}"
    elif [[ "$path" == "~" ]]; then
        path="$HOME"
    fi
    echo "$path"
}

# 检查Python脚本文件
check_python_script() {
    local script_path="$1"
    local service_name="$2"
    
    # 展开路径
    script_path=$(expand_path "$script_path")
    
    echo -e "  检查Python脚本: ${CYAN}$script_path${NC}"
    
    if [ -f "$script_path" ]; then
        echo -e "    ${GREEN}✓ 文件存在${NC}"
        
        # 检查文件是否可执行
        if [ -x "$script_path" ]; then
            echo -e "    ${GREEN}✓ 文件可执行${NC}"
        else
            echo -e "    ${YELLOW}⚠ 文件不可执行，但可以通过python3运行${NC}"
        fi
        
        # 检查Python语法
        if python3 -m py_compile "$script_path" 2>/dev/null; then
            echo -e "    ${GREEN}✓ Python语法检查通过${NC}"
        else
            echo -e "    ${RED}✗ Python语法检查失败${NC}"
            return 1
        fi
        
        return 0
    else
        echo -e "    ${RED}✗ 文件不存在${NC}"
        return 1
    fi
}

# 检查ROS包和节点
check_ros_package() {
    local package="$1"
    local node="$2"
    local service_name="$3"
    
    echo -e "  检查ROS包: ${CYAN}$package${NC}"
    
    # 检查包是否存在
    if rospack find "$package" &>/dev/null; then
        local package_path=$(rospack find "$package")
        echo -e "    ${GREEN}✓ 包存在: $package_path${NC}"
        
        # 如果指定了节点，检查节点文件
        if [ -n "$node" ]; then
            echo -e "  检查ROS节点: ${CYAN}$node${NC}"
            
            # 尝试在包的常见目录中查找节点
            local found=false
            for dir in "scripts" "src" "bin" "nodes"; do
                local node_path="$package_path/$dir/$node"
                if [ -f "$node_path" ]; then
                    echo -e "    ${GREEN}✓ 节点文件存在: $node_path${NC}"
                    found=true
                    break
                fi
            done
            
            if [ "$found" = false ]; then
                echo -e "    ${RED}✗ 节点文件未找到${NC}"
                return 1
            fi
        fi
        
        return 0
    else
        echo -e "    ${RED}✗ 包不存在或未在ROS路径中${NC}"
        return 1
    fi
}

# 检查launch文件
check_launch_file() {
    local package="$1"
    local launch_file="$2"
    local service_name="$3"
    
    echo -e "  检查Launch文件: ${CYAN}$launch_file${NC}"
    
    # 检查包是否存在
    if rospack find "$package" &>/dev/null; then
        local package_path=$(rospack find "$package")
        local launch_path="$package_path/launch/$launch_file"
        
        if [ -f "$launch_path" ]; then
            echo -e "    ${GREEN}✓ Launch文件存在: $launch_path${NC}"
            return 0
        else
            echo -e "    ${RED}✗ Launch文件不存在: $launch_path${NC}"
            return 1
        fi
    else
        echo -e "    ${RED}✗ 包不存在或未在ROS路径中${NC}"
        return 1
    fi
}

# 使用Python解析YAML并检查服务
check_services_with_python() {
    python3 << 'EOF'
import yaml
import sys
import os
import subprocess

def expand_path(path):
    """展开波浪号路径"""
    if path.startswith('~/'):
        return os.path.expanduser(path)
    elif path == '~':
        return os.path.expanduser('~')
    return path

def check_file_exists(path, service_name, file_type):
    """检查文件是否存在"""
    expanded_path = expand_path(path)
    print(f"  检查{file_type}: \033[0;36m{expanded_path}\033[0m")
    
    if os.path.isfile(expanded_path):
        print(f"    \033[0;32m✓ 文件存在\033[0m")
        
        # 检查Python文件的语法
        if file_type == "Python脚本" and expanded_path.endswith('.py'):
            try:
                with open(expanded_path, 'r') as f:
                    compile(f.read(), expanded_path, 'exec')
                print(f"    \033[0;32m✓ Python语法检查通过\033[0m")
            except SyntaxError as e:
                print(f"    \033[0;31m✗ Python语法错误: {e}\033[0m")
                return False
            except Exception as e:
                print(f"    \033[0;33m⚠ 无法检查语法: {e}\033[0m")
        
        return True
    else:
        print(f"    \033[0;31m✗ 文件不存在\033[0m")
        return False

def check_ros_package(package_name):
    """检查ROS包是否存在"""
    try:
        result = subprocess.run(['rospack', 'find', package_name], 
                              capture_output=True, text=True, timeout=10)
        if result.returncode == 0:
            package_path = result.stdout.strip()
            print(f"    \033[0;32m✓ ROS包存在: {package_path}\033[0m")
            return True, package_path
        else:
            print(f"    \033[0;31m✗ ROS包不存在或未在ROS路径中\033[0m")
            return False, None
    except subprocess.TimeoutExpired:
        print(f"    \033[0;33m⚠ rospack命令超时\033[0m")
        return False, None
    except FileNotFoundError:
        print(f"    \033[0;33m⚠ rospack命令未找到，请确保ROS环境已配置\033[0m")
        return False, None
    except Exception as e:
        print(f"    \033[0;33m⚠ 检查ROS包时出错: {e}\033[0m")
        return False, None

def check_ros_node(package_path, node_name):
    """检查ROS节点文件是否存在"""
    # 常见的节点文件位置
    possible_dirs = ['scripts', 'src', 'bin', 'nodes']
    
    for dir_name in possible_dirs:
        node_path = os.path.join(package_path, dir_name, node_name)
        if os.path.isfile(node_path):
            print(f"    \033[0;32m✓ 节点文件存在: {node_path}\033[0m")
            # 检查是否可执行
            if os.access(node_path, os.X_OK):
                print(f"    \033[0;32m✓ 节点文件可执行\033[0m")
            else:
                print(f"    \033[0;33m⚠ 节点文件不可执行，可能需要chmod +x\033[0m")
            return True
    
    print(f"    \033[0;31m✗ 节点文件未找到\033[0m")
    print(f"    \033[0;33m  已搜索目录: {', '.join(possible_dirs)}\033[0m")
    return False

def check_launch_file(package_path, launch_file_name):
    """检查launch文件是否存在"""
    launch_path = os.path.join(package_path, 'launch', launch_file_name)
    
    if os.path.isfile(launch_path):
        print(f"    \033[0;32m✓ Launch文件存在: {launch_path}\033[0m")
        
        # 简单检查launch文件是否为有效的XML
        try:
            import xml.etree.ElementTree as ET
            ET.parse(launch_path)
            print(f"    \033[0;32m✓ Launch文件XML格式有效\033[0m")
        except ET.ParseError as e:
            print(f"    \033[0;31m✗ Launch文件XML格式错误: {e}\033[0m")
            return False
        except Exception as e:
            print(f"    \033[0;33m⚠ 无法验证Launch文件格式: {e}\033[0m")
        
        return True
    else:
        print(f"    \033[0;31m✗ Launch文件不存在: {launch_path}\033[0m")
        return False

# 读取配置文件
config_file = sys.argv[1] if len(sys.argv) > 1 else 'config/config.yaml'

try:
    with open(config_file, 'r', encoding='utf-8') as f:
        config = yaml.safe_load(f)
except Exception as e:
    print(f"\033[0;31m错误：无法读取配置文件: {e}\033[0m")
    sys.exit(1)

if 'services' not in config:
    print(f"\033[0;31m错误：配置文件中没有找到services节点\033[0m")
    sys.exit(1)

services = config['services']
total_services = len(services)
valid_services = 0
invalid_services = 0

print(f"\033[0;34m开始检查 {total_services} 个服务...\033[0m")
print()

for i, service in enumerate(services, 1):
    service_name = service.get('name', f'未命名服务_{i}')
    service_type = service.get('type', '未知')
    
    print(f"\033[0;35m[{i}/{total_services}] 服务: {service_name}\033[0m")
    print(f"  类型: \033[0;36m{service_type}\033[0m")
    
    service_valid = True
    
    if service_type == 'python':
        script_path = service.get('script_path')
        if script_path:
            if not check_file_exists(script_path, service_name, "Python脚本"):
                service_valid = False
        else:
            print(f"    \033[0;31m✗ 未指定script_path\033[0m")
            service_valid = False
            
    elif service_type == 'rosrun':
        package = service.get('package')
        node = service.get('node')
        if package and node:
            print(f"  检查ROS包: \033[0;36m{package}\033[0m")
            package_exists, package_path = check_ros_package(package)
            
            if package_exists:
                print(f"  检查ROS节点: \033[0;36m{node}\033[0m")
                if not check_ros_node(package_path, node):
                    service_valid = False
            else:
                service_valid = False
        else:
            print(f"    \033[0;31m✗ 缺少package或node配置\033[0m")
            service_valid = False
            
    elif service_type == 'roslaunch':
        package = service.get('package')
        launch_file = service.get('launch_file')
        if package and launch_file:
            print(f"  检查ROS包: \033[0;36m{package}\033[0m")
            package_exists, package_path = check_ros_package(package)
            
            if package_exists:
                print(f"  检查Launch文件: \033[0;36m{launch_file}\033[0m")
                if not check_launch_file(package_path, launch_file):
                    service_valid = False
            else:
                service_valid = False
        else:
            print(f"    \033[0;31m✗ 缺少package或launch_file配置\033[0m")
            service_valid = False
    else:
        print(f"    \033[0;33m⚠ 未知的服务类型: {service_type}\033[0m")
    
    if service_valid:
        valid_services += 1
        print(f"  \033[0;32m✓ 服务配置有效\033[0m")
    else:
        invalid_services += 1
        print(f"  \033[0;31m✗ 服务配置无效\033[0m")
    
    print()

# 输出总结
print(f"\033[0;34m======================================\033[0m")
print(f"\033[0;34m    检查结果总结\033[0m")
print(f"\033[0;34m======================================\033[0m")
print(f"总服务数: {total_services}")
print(f"\033[0;32m有效服务: {valid_services}\033[0m")
print(f"\033[0;31m无效服务: {invalid_services}\033[0m")

if invalid_services == 0:
    print(f"\033[0;32m🎉 所有服务文件检查通过！\033[0m")
    sys.exit(0)
else:
    print(f"\033[0;31m❌ 发现 {invalid_services} 个服务存在问题，请检查上述错误信息\033[0m")
    sys.exit(1)
EOF
}

# 主函数
main() {
    # 解析命令行参数
    VERBOSE=false
    CONFIG_PATH="$CONFIG_FILE"
    
    while [[ $# -gt 0 ]]; do
        case $1 in
            -v|--verbose)
                VERBOSE=true
                shift
                ;;
            -c|--config)
                CONFIG_PATH="$2"
                shift 2
                ;;
            -h|--help)
                echo "用法: $0 [选项]"
                echo ""
                echo "选项:"
                echo "  -v, --verbose    详细输出模式"
                echo "  -c, --config     指定配置文件路径 (默认: config/config.yaml)"
                echo "  -h, --help       显示此帮助信息"
                echo ""
                echo "示例:"
                echo "  $0                    # 使用默认配置文件检查"
                echo "  $0 -v                 # 详细模式检查"
                echo "  $0 -c custom.yaml     # 使用自定义配置文件"
                exit 0
                ;;
            *)
                echo -e "${RED}未知参数: $1${NC}"
                echo "使用 -h 或 --help 查看帮助"
                exit 1
                ;;
        esac
    done
    
    # 检查依赖
    check_dependencies
    
    # 检查ROS环境
    check_ros_environment
    
    echo ""
    echo -e "${BLUE}开始检查服务文件...${NC}"
    echo ""
    
    # 使用Python解析YAML并检查服务
    check_services_with_python "$CONFIG_PATH"
    
    exit_code=$?
    
    echo ""
    if [ $exit_code -eq 0 ]; then
        echo -e "${GREEN}======================================${NC}"
        echo -e "${GREEN}    所有检查完成，未发现问题！${NC}"
        echo -e "${GREEN}======================================${NC}"
    else
        echo -e "${RED}======================================${NC}"
        echo -e "${RED}    检查完成，发现问题！${NC}"
        echo -e "${RED}======================================${NC}"
        echo ""
        echo -e "${YELLOW}建议：${NC}"
        echo "1. 检查文件路径是否正确"
        echo "2. 确保所有Python脚本语法正确"
        echo "3. 验证ROS包是否已正确安装"
        echo "4. 检查文件权限设置"
    fi
    
    exit $exit_code
}

# 运行主函数
main "$@" 